/**
  ******************************************************************************
  * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
  * @file    LegController.h
  * @author   
  * @brief   
  ******************************************************************************
  */
#ifndef LEGCONTROLLER_H
#define LEGCONTROLLER_H

/* Includes ------------------------------------------------------------------*/
#include <iostream>
#include "common/DataType/RobotCtrlType.h"
#include "common/Dynamics/Quadruped.h"
/* Private macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
/* Exported function declarations --------------------------------------------*/	
typedef enum{
  All_Tau,
  All_Pos,
  Abad_Pos
} Ctrl_Mode;

/*!
 * Data sent from the control algorithm to the legs.
 */
template <typename T>
struct LegControllerCommand {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  LegControllerCommand() { zero(); }

  void zero();

  Vec3<T> tauFeedForward, forceFeedForward, qDes, qdDes, pDes, vDes;
  Mat3<T> kpCartesian, kdCartesian, kpJoint, kdJoint;
};

/*!
 * Data returned from the legs to the control code.
 */
template <typename T>
struct LegControllerData {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  LegControllerData() { zero(); }

  void setQuadruped(Quadruped<T>& quad) { quadruped = &quad; }
  void zero();

  Vec3<T> q, qd, p, v;
  Mat3<T> J;
  Vec3<T> tauEstimate;
  Quadruped<T>* quadruped;
};

/*!
 * Controller for 4 legs of a quadruped
 */
template <typename T>
class LegController {
 public:
  LegController(Quadruped<T>& quad) : _quadruped(quad) {
    for (auto& data : datas) data.setQuadruped(_quadruped);
  }

  void zeroCommand();
  void edampCommand(T gain);
  void updateData(const LegData* robotData);
  void updateCommand(RobotCommand* robotCommand);
  void setEnabled(bool enabled) { 
      for(int leg = 0; leg < 4; leg ++) _legsEnabled[leg] = enabled; 
    };
  void setEnabled(int leg, bool enabled){ _legsEnabled[leg] = true;}
  void setControlMode(Ctrl_Mode mode);
  // void setLcm(leg_control_data_lcmt* data, leg_control_command_lcmt* command);

  LegControllerCommand<T> commands[4];
  LegControllerData<T> datas[4];
  Quadruped<T>& _quadruped;
  bool _legsEnabled[4] = {false, false, false, false};
  float debug_data[12];
  private:
  Leg_Mode ctrl_mode[3] = {Mode_Pos, Mode_Pos, Mode_Pos};
};

template <typename T>
void computeAuxiliaryAngle(Quadruped<T>& quad, Vec3<T>& p, Vec3<T>* q);

template <typename T>
void computeLegAngle(Quadruped<T>& quad, Vec3<T>& p, Vec3<T>* q);

template <typename T>
void computeLegJacobianAndPosition(Quadruped<T>& quad, Vec3<T>& q, Mat3<T>* J,
                                   Vec3<T>* p);
                                
template <typename T>
void computeLegPosition(Quadruped<T>& quad, Vec3<T>& q, Vec3<T>* p);                                

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
 





